
#include <armadillo>
class EP_tg
{
	public:

	EP_tg(void)
	{	
		boost::math::normal_distribution<> *d=new boost::math::normal_distribution<>(0,1); 
		_d=d;
	}
	~EP_tg()
	{
		delete _d;
	}

	void operator()(mat m,mat Sig,mat b,int npass,double alpha=1)
	{
		int p=Sig.n_cols;
		mat Qmui(p,p);
		mat mmi(p,1);
		mat Q(p,p);
		Q=inv(Sig);
		cube Qi(p,p,p);
		mat Qmi(p,p);
		Qmui.fill(0);
		Qi.fill(0);
		mat e(p,1);
		for(int k=0;k<npass;k++)
		{
			for(int i=0;i<p;i++)
			{
				mat oldmu=m;
				mat oldQ=Q;
				mat temp=Qi(span(i,i),span::all,span::all);
				Qmi=Q-temp;
				mmi=inv(Qmi)*(Q*m-Qmui(i,span::all).t());
				e.fill(0);
				e(i,0)=1;
				mat Vmi=inv(Qmi);
				double si=Vmi(i,i);
				//cout << "si" << si << "\n";
				mat dlZm=(pdf(*_d,(b(i,0)-mmi(i,0))/sqrt(si))/(sqrt(si)*(1-cdf(*_d,(b(i,0)-mmi(i,0))/sqrt(si)))))*e;
			//	cout << "dm" << dlZm;	
				mat dlZS=(((double)1/((1-cdf(*_d,(b(i,0)-mmi(i,0))/sqrt(si)))))*pdf(*_d,(b(i,0)-mmi(i,0))/sqrt(si))*(b(i,0)-mmi(i,0))/(sqrt(si)*si))*e*e.t();
				//cout << "dS " << dlZS;	
				m=mmi+Vmi*dlZm;
				mat V=Vmi-Vmi*(dlZm*dlZm.t()-2*dlZS)*Vmi;
				cout << "V" << V; 
				Q=inv(V);
				mat T=Qi(span(i,i),span::all,span::all);
				Qi(span(i,i),span::all,span::all)=alpha*(Q-Qmi)+(1-alpha)*T;;
				//temp=Qi(span(i,i),span::all,span::all);
				//cout << "temp" << temp;
				Qmui(span::all,i)=(alpha*(Q*m-Qmi*mmi))+(1-alpha)*Qmui(span::all,i);
			}
		
		}
		_m=m;
		_S=inv(Q);
		//cout << _S;

	}
	mat Get_Sig(void){return _S;}
	mat Get_Mu(void){return _m;}
	private:
	mat _m;
	mat _S;
	boost::math::normal_distribution<> *_d; 
};

